/*
 * @file test_adafruit_motorshieldv2.cpp
 * @brief Adafruit 电机驱动板 v2 库的测试
 * 
 * 本文件用于测试 Adafruit 步进电机 + 直流电机 FeatherWing
 * 参考链接：https://learn.adafruit.com/adafruit-stepper-dc-motor-featherwing
 * 
 * 测试基于以下示例：
 * https://github.com/adafruit/Adafruit_Motor_Shield_V2_Library/blob/master/examples/DCMotorTest/DCMotorTest.ino
 * 
 * 作者：Franz Pucher
 */

// 包含 Unity 库
// 详情请参见 https://docs.platformio.org/en/latest/plus/unit-testing.html#api
#include <unity.h>

// 使用 Arduino 的 Wire 库进行 I2C 通信
#include <Wire.h>
#include <Adafruit_MotorShield.h>

#include "diffbot_base_config.h"

// 创建电机驱动板对象，使用默认的 I2C 地址
Adafruit_MotorShield AFMS = Adafruit_MotorShield(MOTOR_DRIVER_ADDR);
// 或者使用不同的 I2C 地址（例如用于堆叠）
// Adafruit_MotorShield AFMS = Adafruit_MotorShield(0x61); 

// 选择 'port' M1, M2, M3 或 M4。在此示例中，选择 M1
Adafruit_DCMotor *rightMotor = AFMS.getMotor(MOTOR_RIGHT);
// 也可以在 M2 端口上创建另一个电机
Adafruit_DCMotor *leftMotor = AFMS.getMotor(MOTOR_LEFT);

/**
 * @brief 电机速度扫频测试
 * 
 * @param motor 要测试的电机对象
 */
void motorSweep(Adafruit_DCMotor *motor)
{
  uint8_t i;
  motor->run(FORWARD);
  for (i=0; i<255; i++) {
    motor->setSpeed(i);  
    delay(10);
  }
  for (i=255; i!=0; i--) {
    motor->setSpeed(i);  
    delay(10);
  }
  
  Serial.print("tock");

  motor->run(BACKWARD);
  for (i=0; i<255; i++) {
    motor->setSpeed(i);  
    delay(10);
  }
  for (i=255; i!=0; i--) {
    motor->setSpeed(i);  
    delay(10);
  }

  Serial.print("tech");
  motor->run(RELEASE);
  delay(1000);
}

/**
 * @brief 初始化设置
 */
void setup() {
  // 注意！！！等待 >2 秒
  // 如果板子不支持通过 Serial.DTR/RTS 进行软件复位
  delay(2000);
  UNITY_BEGIN();

  Serial.begin(9600);           // 设置串口通信，波特率为 9600 bps
  Serial.println("Adafruit Motorshield v2 - DC Motor test!");

  AFMS.begin();  // 使用默认频率 1.6KHz 初始化电机驱动板
  //AFMS.begin(1000);  // 或者使用不同的频率，例如 1KHz
  
  // 设置初始速度，从 0（关闭）到 255（最大速度）
  Serial.println("setSpeed(150)");
  leftMotor->setSpeed(150);
  leftMotor->run(FORWARD);
  // 打开电机
  leftMotor->run(RELEASE);

  // 设置初始速度，从 0（关闭）到 255（最大速度）
  rightMotor->setSpeed(150);
  rightMotor->run(FORWARD);
  // 打开电机
  rightMotor->run(RELEASE);
}

uint8_t cycle = 0;
uint8_t max_loop_cycles = 1;

/**
 * @brief 主循环
 */
void loop() {

  if (cycle < max_loop_cycles)
  {
    Serial.print("tick");

    motorSweep(leftMotor);
    motorSweep(rightMotor);
  }
  else
  {
    leftMotor->setSpeed(0); 
    leftMotor->run(RELEASE);
    rightMotor->setSpeed(0); 
    rightMotor->run(RELEASE);
    delay(1000);
    UNITY_END();
  }

  cycle++;
}